/** @file circler.cpp
  *
  * Funkcje skladowe klasy Circle.
  */

#include "circle.h"

const Vector2 g(0, 9.81); ///@todo Przeniesc do stalych

Circle::Circle(const Vector2 &o, const int &r_, const int &c) :
        Object(o, c), r(r_), m(static_cast<float>(r_)/MASS_FACTOR), alfa(0), omega(0) {}

/** @fn void Circle::deltaT(const float &dt, const unsigned int &W, const unsigned int &H)
  * @param dt Odcinek czasu
  * @param W Szerokosc ekranu
  * @param H Wysokosc ekranu
  * @bug Male okregi dziwnie drgaja.
  * @brief Funkcja oblicza parametry kola po jego przesunieciu, oraz oblicza odbicia od scianek
  *
  */
void Circle::deltaT(const float &dt, const unsigned int &W, const unsigned int &H, bool doGravity) {
  if (v.mag() >= 0.0001)
    f -= ((v*AIR_RES)/v.mag());

  /// Ruch postepowy
  a = (f/m);
  if(doGravity) a+=g*10;
  v += (a*dt);
  p += (v*dt);
  f = Vector2(0,0);

  /// Ruch obrotowy
  alfa += (omega*dt);

  /// Odbijanie sie od scianek.
  /// @todo Wprowadzic plane.
  if ((p.x+r >= W) && (v.x > 0))
    v.x =- (v.x * BOUNCE_FLEX);
  else if ((p.x-r <= 0) && (v.x < 0))
    v.x =- (v.x * BOUNCE_FLEX);
  if((p.y+r >= H) && (v.y >= 0)) {
    v.y =- (v.y * BOUNCE_FLEX);
    omega = 0.0;
  }
  else if ((p.y-r <= 0) && (v.y <= 0))
    v.y =- (v.y * BOUNCE_FLEX);

  /// Dodatkowe zabezpieczenie.
  if((p.y+r) >= H) p.y = H-r;
  else if((p.y-r) <= 0) p.y = r;

  if((p.x+r)>=W) p.x = W-r;
  else if((p.x-r) <= 0) p.x = r;
}

/** @fn void Circle::collision(Circle &a)
  * @param a Kolo z ktorym obliczamy zderzenie
  * @bug Cos nie tak z tarciem, kiedy jest g.
  * @brief Funkcja sprawdza odleglosc od zadanego kola a nastepnie w razie kolizji oblicza nowe parametry.
  *
  */
void Circle::collision(Circle &a) {
  float d = dist(p, a.p);
  float dr = d - (r + a.r);

  if(dr < 0){
    Vector2 v1 = v, v2 = a.v;
    float m1 = m, m2 = a.m;

    // wektor jednostkowy normalny
    Vector2 un ((p-a.p)/dist(p, a.p));

    // wektor jednostkowy styczny
    Vector2 ut (-un.y, un.x);

    // skladowe predkosci
    float v1n = dotp(v1, un);
    float v1t = dotp(v1, ut);
    float v2n = dotp(v2, un);
    float v2t = dotp(v1, ut);

    // ZZP
    float v1np = (v1n*(m1-m2) + 2*m2*v2n) / (m1+m2);
    float v2np = (v2n*(m2-m1) + 2*m1*v1n) / (m1+m2);

    // Przeliczenia
    v = (un*v1np) + (ut*v1t);
    a.v = (un*v2np) + (ut*v2t);

    // Odbicie
    v *= BOUNCE_FLEX;
    a.v *= BOUNCE_FLEX;

    // TYMCZASOWA obsluga toczenia
    /* Do wektora predkosci dodajemy zmiane prostopadla do powierzchni w pkt styku kulek
    o wartosci zaleznej od predkosci katowej //*/
    static const float tmpFactor = 10.0; // nie wiem czy to bedzie ostatecznie potrzebne
    v += ut*-omega*(a.m/(m+a.m))*tmpFactor;
    a.v += ut*-a.omega*(m/(m+a.m))*tmpFactor;
    omega *= 1.1;
    a.omega *= 1.1;

    // Bezpiecznik!
    Vector2 x = p - a.p;
    a.p += (x/d) * dr;
    p -= (x/d) * (dr/2);

  }
}
